/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "camera_service.h"

#include "camera_service_impl.h"
// #include <Eigen/Geometry>
#include <string.h>
#include <yaml-cpp/yaml.h>

#include <iostream>
// #include "glog/logging.h"
#include "base/common/log.h"
#include "base/io/file_util.h"

namespace airos {
namespace middleware {
namespace device_service {

using CameraImageData = base::device::CameraImageData;

CameraService::CameraService() : impl_(new CameraServiceImpl()) {}

bool CameraService::Init(const base::device::CameraInitConfig &config) {
  return impl_->Init(config);
}

std::shared_ptr<CameraImageData> CameraService::GetCameraData(
    unsigned int timeout_ms) {
  return impl_->GetCameraData(timeout_ms);
}

CameraService::~CameraService() = default;

// bool CameraService::LoadExtrinsics(const std::string &yaml_file,
// Eigen::Matrix4d& extrinsic) {
bool CameraService::LoadExtrinsics(const std::string &yaml_file,
                                   Eigen::Affine3d &camera2world_pose) {
  airos::base::FileUtil file_util;
  if (!file_util.Exists(yaml_file)) {
    LOG_INFO << yaml_file << " not exist!";
    return false;
  }

  YAML::Node node = YAML::LoadFile(yaml_file);
  double qw = 0.0;
  double qx = 0.0;
  double qy = 0.0;
  double qz = 0.0;
  double tx = 0.0;
  double ty = 0.0;
  double tz = 0.0;
  std::string trans_frame_id, trans_child_frame_id;
  try {
    if (node.IsNull()) {
      LOG_INFO << "Load " << yaml_file << " failed! please check!";
      return false;
    }
    qw = node["transform"]["rotation"]["w"].as<double>();
    qx = node["transform"]["rotation"]["x"].as<double>();
    qy = node["transform"]["rotation"]["y"].as<double>();
    qz = node["transform"]["rotation"]["z"].as<double>();
    tx = node["transform"]["translation"]["x"].as<double>();
    ty = node["transform"]["translation"]["y"].as<double>();
    tz = node["transform"]["translation"]["z"].as<double>();
    trans_frame_id = node["header"]["frame_id"].as<std::string>();
    trans_child_frame_id = node["child_frame_id"].as<std::string>();
  } catch (YAML::InvalidNode &in) {
    LOG_ERROR << "load camera extrisic file " << yaml_file
              << " with error, YAML::InvalidNode exception";
    return false;
  } catch (YAML::TypedBadConversion<double> &bc) {
    LOG_ERROR << "load camera extrisic file " << yaml_file
              << " with error, YAML::TypedBadConversion exception";
    return false;
  } catch (YAML::Exception &e) {
    LOG_ERROR << "load camera extrisic file " << yaml_file
              << " with error, YAML exception:" << e.what();
    return false;
  }
  // extrinsic.setConstant(0);
  Eigen::Quaterniond Q_temp(qw, qx, qy, qz);
  Eigen::Matrix3d rotation_world2camera = Q_temp.matrix().transpose();
  Eigen::Vector3d t_camera2world(tx, ty, tz);
  Eigen::Translation3d translation(t_camera2world(0, 0), t_camera2world(1, 0),
                                   t_camera2world(2, 0));
  camera2world_pose = translation * rotation_world2camera.transpose();
  // extrinsic.block<3, 3>(0, 0) = Q_temp.normalized().toRotationMatrix();
  // extrinsic(0, 3) = tx;
  // extrinsic(1, 3) = ty;
  // extrinsic(2, 3) = tz;
  // extrinsic(3, 3) = 1;
  //   if (frame_id != nullptr) {
  //     *frame_id = trans_frame_id;
  //   }
  //   if (child_frame_id != nullptr) {
  //     *child_frame_id = trans_child_frame_id;
  //   }
  return true;
}

bool CameraService::LoadIntrinsics(const std::string &yaml_file,
                                   Eigen::Matrix3f &intrinsic_params) {
  airos::base::FileUtil file_util;
  if (!file_util.Exists(yaml_file)) {
    return false;
  }

  YAML::Node node = YAML::LoadFile(yaml_file);
  if (node.IsNull()) {
    LOG_INFO << "Load " << yaml_file << " failed! please check!";
    return false;
  }

  float camera_width = 0;
  float camera_height = 0;
  Eigen::VectorXf params(9 + 5);
  try {
    camera_width = node["width"].as<float>();
    camera_height = node["height"].as<float>();
    for (size_t i = 0; i < 9; ++i) {
      params(i) = node["K"][i].as<float>();
    }
    for (size_t i = 0; i < 5; ++i) {
      params(9 + i) = node["D"][i].as<float>();
    }
  } catch (YAML::Exception &e) {
    LOG_ERROR << "load camera intrisic file " << yaml_file
              << " with error, YAML exception: " << e.what();
    return false;
  }
  intrinsic_params(0, 0) = params(0);
  intrinsic_params(0, 1) = params(1);
  intrinsic_params(0, 2) = params(2);
  intrinsic_params(1, 0) = params(3);
  intrinsic_params(1, 1) = params(4);
  intrinsic_params(1, 2) = params(5);
  intrinsic_params(2, 0) = params(6);
  intrinsic_params(2, 1) = params(7);
  intrinsic_params(2, 2) = params(8);
  return true;
}

bool CameraService::LoadGroundPlaneCoffe(const std::string &yaml_file,
                                         Eigen::Vector4d &ground_plane_coffe) {
  airos::base::FileUtil file_util;
  if (!file_util.Exists(yaml_file)) {
    LOG_ERROR << yaml_file << " not exist! please check!";
    return false;
  }
  YAML::Node node = YAML::LoadFile(yaml_file);
  if (node.IsNull()) {
    LOG_ERROR << "Load " << yaml_file << " failed! please check!";
    return false;
  }
  try {
    ground_plane_coffe(0, 0) = node["a"].as<float>();
    ground_plane_coffe(1, 0) = node["b"].as<float>();
    ground_plane_coffe(2, 0) = node["c"].as<float>();
    ground_plane_coffe(3, 0) = node["d"].as<float>();
  } catch (YAML::Exception &e) {
    LOG_ERROR << "load camera ground file " << yaml_file
              << " with error, YAML exception: " << e.what();
    return false;
  }
  return true;
}

}  // namespace device_service
}  // namespace middleware
}  // namespace airos